package AStar;

import java.awt.Point;
import java.util.ArrayList;
import java.util.PriorityQueue;
import model.Node;

/**
 *
 * @author Carlos Henrique Silva Galdino
 * @author Raphael Pereira de Faria
 *
 */
public class AStar {

    private PriorityQueue<Node> open;
    private ArrayList<Node> closed;
    private Grid grid;

    public AStar(Grid grid) {
        this.grid = grid;
    }

    /**
     * Método que realiza a busca do caminho de menor custo utilizando o algoritmo A*
     * @param grid - grid contendo a disposição dos pontos
     * @return Nó anterior ao nó final, ou nulo, caso não exista um caminho
     */
    public Node pathFind(Grid grid) {

        open = new PriorityQueue<Node>();
        closed = new ArrayList<Node>();

        open.clear();
        closed.clear();

        Node current = grid.getNohInicial();

        Node goal = grid.getNohFinal();
        calculaF_n(current);
        open.add(current);
        while (!open.isEmpty()) {
            current = open.poll();
            closed.add(current);
            if (current.equals(goal)) {
                break;
            } else {
                ArrayList<Node> irmaos = getIrmaos(current);
                for (Node noh : irmaos) {
                    Node nodeSameCoord = isInOpen(noh);
                    if (nodeSameCoord != null) {
                        if (nodeSameCoord.getCustoTotal() > noh.getCustoTotal()) {
                            open.remove(nodeSameCoord);
                            calculaF_n(noh);
                            open.offer(noh);
                        }
                    } else if (!isInClosed(noh)) {
                        if (!noh.isObstaculo()) {
                            calculaF_n(noh);
                            open.offer(noh);
                        }
                    }
                }
            }
        }
        if (current.equals(grid.getNohFinal())) {
            return current.getPai();
        } else {
            return null;
        }
    }

    public void reconstructPath(Node noh) {
        if (noh != null) {
            reconstructPath(noh.getPai());
        }
    }

    private ArrayList<Node> getClosed() {
        return closed;
    }

    private PriorityQueue<Node> getOpen() {
        return open;
    }

    private Node isInOpen(Node noh) {
        for (Node n : getOpen()) {
            if (noh.equals(n)) {
                return n;
            }
        }
        return null;
    }

    private boolean isInClosed(Node noh) {
        for (Node n : getClosed()) {
            if (noh.equals(n)) {
                return true;
            }
        }
        return false;
    }

    private ArrayList<Node> getIrmaos(Node noh) {
        return grid.getIrmaos(noh);
    }

    private void calculaF_n(Node noh) {
        noh.setF_n(heuristica(noh));
    }

    public int heuristica(Node noAtual) {
        return heuristica(noAtual.getCoord(), getNohFinal().getCoord());
    }

    private int heuristica(Point pontoAtual, Point pontoFinal) {
        //return diagonalShortcut(pontoAtual.x, pontoAtual.y, pontoFinal.x, pontoFinal.y);
        return distanciaManhattan(pontoAtual.x, pontoAtual.y, pontoFinal.x, pontoFinal.y);

    }

    ///////heuristicas
    private int distanciaManhattan(int x1, int y1, int x2, int y2) {
        return Math.abs(x1 - x2) + Math.abs(y1 - y2);
    }

    private int distanciaEuclidiana(int x1, int y1, int x2, int y2) {
        return (int) (Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)));
    }

    private double diagonalShortcut(int x1, int y1, int x2, int y2) {
        Point start = grid.getNohInicial().getCoord();
        double dx1 = x1 - x2;
        double dy1 = y1 - y2;
        double dx2 = start.x - x2;
        double dy2 = start.y - x2;
        double cross = dx1 * dy2 - dx2 * dy1;
        if (cross < 0) {
            cross = -cross;
        }

        return 1.1 * (Math.abs(dx1) + Math.abs(dy1) + cross * 0.0001);
    }

    public boolean isNohFinal(Node noh) {
        return noh.equals(this.getNohFinal());
    }

    public Node getNohFinal() {
        return grid.getNohFinal();
    }

    public Node getNohInicial() {
        return grid.getNohInicial();
    }
}
